#include<math.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>

#define STATUS_EXPLORING    (0)
#define STATUS_CLOSE_TARGET (1)
#define STATUS_GO_HOME      (2)

#define GET_TARGET_SIZE     (90000)

ros::Publisher vel_pub;
ros::Publisher cmd_pub;
ros::Publisher voice_pub;

int status_flag = STATUS_EXPLORING;

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
    // // 将接收到的消息打印出来
    // if(msg->position.z < GET_TARGET_SIZE){
    //     ROS_INFO("Target_Pose: z:%0.6f", msg->position.z);
    // }
    

    /******************************请补充此处代码（开始）*******************************************/

    //停止机器人导航
    if(status_flag == STATUS_EXPLORING)
    {
        status_flag = STATUS_CLOSE_TARGET;
        std_msgs::Int8 cmd;
        cmd.data = STATUS_CLOSE_TARGET;
        ROS_INFO("Find maze!!!");
        cmd_pub.publish(cmd);

        std_msgs::String msg;
        msg.data = "发现宝藏,向宝藏进发";
        voice_pub.publish(msg);
    }
    else if(status_flag == STATUS_CLOSE_TARGET && msg->position.z > 90000)
    {
        status_flag = STATUS_GO_HOME;
        std_msgs::Int8 cmd;
        cmd.data = STATUS_GO_HOME;
        cmd_pub.publish(cmd);
        ROS_INFO("Get maze, back!!!");

        std_msgs::String msg;
        msg.data = "宝藏已到手,返回基地";
        voice_pub.publish(msg);
    }
    else if(status_flag == STATUS_CLOSE_TARGET)
    {
        //初始化geometry_msgs::Twist类型的消息
        geometry_msgs::Twist vel_msg;
        //设置线速度
        vel_msg.linear.x = (100000-msg->position.z)/100000*0.3;
        //设置角速度
        vel_msg.angular.z = (640-msg->position.x)/640*0.3;
        //发布消息s
        vel_pub.publish(vel_msg);
        ROS_INFO("move close");
    }
    /******************************请补充此处代码（结束）*******************************************/
}

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "move_to_target");

	// 创建节点句柄
	ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/object_detect_pose", 10, poseCallback);
	// 创建一个Publisher，发布名为cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
	// 创建一个Publisher，发布名为cmd_vel的topic，消息类型为std_msgs::Int8，队列长度10
	cmd_pub = n.advertise<std_msgs::Int8>("/exploring_cmd", 10);
    // 发布语音输出内容    
    voice_pub = n.advertise<std_msgs::String>("voiceWords", 1000); 

    // 循环等待回调函数
    ros::spin();

	return 0;
}
